#include "gimbal_control_task.h"
#include "DM4310.h"
#include "bsp_dwt.h"
#include "stm32f4xx_hal.h"
#include "vision.h"
#include "remote.h"
#include "imu.h"
#include "gimbal_use.h"
#include "auto_aim.h"
#include "can_controller.h"
/* 内部宏定义 ----------------------------------------------------------------*/

/* 内部自定义数据类型的变量 --------------------------------------------------*/

static float Pitch_Current_Value,Yaw_Current_Value;

static float yaw_angle_get,yaw_angle_set;  	 	
static float yaw_speed_get;	 
static float pitch_angle_get,pitch_angle_set ; 
static float pitch_speed_get;   

//调试变量
static int32_t sin_counter,count;
int8_t rotate_direction;
/* 函数主体部分 --------------------------------------------------------------*/
/**
  * @brief				云台初始化任务，pitch抬到水平在初始化陀螺仪
  * @param[in]		
	* @param[out]		
  * @retval				none
*/

void gimbal_init_task(void const * argument)
{
	// //延时等待CAN启动
	vTaskSuspendAll();
	Motor6020_PID_Init();
	for(;;)
	{
		//零漂处理
    	while (BMI088_init(&hspi1, 1) != BMI088_NO_ERROR){}	
		xTaskResumeAll();	
		vTaskDelete(gimbal_init_TaskHandle);		

	}
}
/**
  * @brief				云台任务
*/
#ifdef GIMBAL_BOARD_DOWN

void gimbal_control_task(void const *pvParameters)
{
	uint32_t cnt_last;
	float dt_gimbal;
	
	//巡逻模式变量	
	 float pitch_patrol_w=3.0f; 	//rad/s  
	 float yaw_patrol_w=-0.0025f;
	 float time_all;
		
	//控制器参数初始化
	Motor6020_PID_Init();
	
	portTickType xLastWakeTime;
  	xLastWakeTime = xTaskGetTickCount();
  /* Infinite loop */
  for(;;)
  	{
		dt_gimbal = DWT_GetDeltaT (&cnt_last);
		sin_counter++;
		
		gimbal_status.gimbal_mode =REMOTE_MODE;			
	

		RemoteControlProcess(RC);			
		//云台模式切换 目标值更新
		Convermode_set(&gimbal_status);			//模式切换间过渡过程		

		rotate_direction = top_direction_judge(minipc_rx.Yaw_speed);
		rotate_flag = top_status_judge(minipc_rx.Yaw_speed);
		gimbal_status.aimcenter_flag=aimcenter_judge(minipc_rx.Yaw_speed*1.2,6.2);

		switch(gimbal_status.gimbal_mode)
		{
			case REMOTE_MODE:
			{			
			//pitch轴限幅
			LimitPitch_encoder(&target_set,7800,7300,motor_param_get[PITCH_MOTOR].offset_angle);
//			target_set.pitch_targetangle_remote=(0.4*sin(sin_counter*0.005));			
					
			/*yaw*/
			yaw_angle_set 		= target_set.yaw_targetangle_remote;		
			yaw_angle_get		= INS.Yaw.total_radangle;
			yaw_speed_get 		= INS.Yaw.speed;
			/*pitch*/
			pitch_angle_set 	= target_set.pitch_targetangle_remote;
			pitch_angle_get		= motor_param_get[PITCH_MOTOR].total_radangle;
			pitch_speed_get		= INS.Pitch.speed;
					
//			Yaw_Current_Value=(2000*sign(sin(sin_counter*0.015)));							
//			Pitch_Current_Value=(2000*sign(sin(sin_counter*0.05)));						
//			/*此处为陀螺仪控制*/
			Yaw_Current_Value = PID_YAW_CAL_forward(&yaw_rc_motor_PIDpos,&yaw_rc_motor_PIDspeed,
								yaw_angle_get,yaw_speed_get,yaw_angle_set,0,0.002,0.0);

			Pitch_Current_Value = PID_PITCH_CAL_forward(&pitch_rc_motor_PIDpos,&pitch_rc_motor_PIDspeed,
								pitch_angle_get,pitch_speed_get,pitch_angle_set,0,0.002,0);					
			}break;
			case PATROL_MODE:
			{
          	/*正常巡逻*/						
			if(gimbal_status.last_gimbal_mode == PATROL_MODE)
			{
				time_all +=0.016;
				//期望值更新
				target_set.yaw_targetangle_patrol	 +=yaw_patrol_w;
				//清圈数
				if(fabs(INS.Yaw.angle_round)>=1)
				{
			    	INS.Yaw.angle_round=0;
		     		//更新total 值			
		      		INS.Yaw.total_radangle=INS.Yaw.ins_angle*angle2radian;						
					target_set.yaw_targetangle_patrol=INS.Yaw.total_radangle;
				}						
//           	target_set.pitch_targetangle_patrol	 =0.153*sin(pitch_patrol_w*time_all);
				target_set.pitch_targetangle_patrol=0.1;
			}

			LimitPitch_encoder(&target_set,7800,7300,motor_param_get[PITCH_MOTOR].offset_angle);							
			/*yaw*/
			yaw_angle_set 		= 	target_set.yaw_targetangle_patrol;
			yaw_angle_get		= 	INS.Yaw.total_radangle;
			yaw_speed_get 		=	INS.Yaw.speed;
			/*pitch*/
			pitch_angle_set 	= 	target_set.pitch_targetangle_patrol	;
			pitch_angle_get		= 	motor_param_get[PITCH_MOTOR].total_radangle;
			pitch_speed_get		= 	INS.Pitch.speed;
					
			/*此处为陀螺仪控制*/
			Yaw_Current_Value = PID_YAW_CAL_forward(&yaw_patrol_motor_PIDpos,&yaw_patrol_motor_PIDspeed,
									yaw_angle_get,yaw_speed_get,yaw_angle_set,0,0.002,0);
			Pitch_Current_Value = PID_PITCH_CAL_forward(&pitch_patrol_motor_PIDpos,&pitch_patrol_motor_PIDspeed,
									pitch_angle_get,pitch_speed_get,pitch_angle_set,0,0.002,0);					
			}break;

			case AUTO_AIM_MODE:
			{		
			if(gimbal_status.last_gimbal_mode ==AUTO_AIM_MODE)	
			{
						

				if(rotate_flag == 0 )
				{
					Woody_Autoaim(real_shoot_speed);		
				}
				else if(rotate_flag == 1 )
				{
					// if(minipc_rx.armor_id==0)
					// {
					// 	Woody_Antioutpost(real_shoot_speed, &ballistic_solusion);
					// }else
					{
						if (gimbal_status.aimcenter_flag==0) 
						{
							Woody_Antitop(real_shoot_speed);							
						}
						else 
						{
							Woody_Aimcentre(real_shoot_speed);
						}
								
					}	
				}	
					
			}				
			else 
			{
				wait_armor = track_armor = 0;
			}
					
			//PITCH轴限位
			LimitPitch_gyro(&target_set,262*coder2radian,-210*coder2radian);
					
			/*yaw*/         
			yaw_angle_set           = target_set.yaw_targetangle_gyro;
			yaw_angle_get			= INS.Yaw.total_radangle ;
			yaw_speed_get 		 	= INS.Yaw.speed;
			/*pitch*/			
			pitch_angle_set         = target_set.pitch_targetangle_gyro;
			pitch_angle_get			= INS.Pitch.total_radangle ;
			pitch_speed_get			= INS.Pitch.speed;

			if(rotate_flag ==0)	
			{
				Yaw_Current_Value = PID_YAW_CAL_forward(&yaw_auto_aim_PIDpos,&yaw_auto_aim_PIDspeed,
									yaw_angle_get,yaw_speed_get,yaw_angle_set,0.05,0.002,0.6);											
				Pitch_Current_Value = PID_PITCH_CAL_forward(&pitch_auto_aim_PIDpos,&pitch_auto_aim_PIDspeed,
									pitch_angle_get,pitch_speed_get,pitch_angle_set,0.05,0.002,0.5);				
			}
			else if(rotate_flag==1 )
			{
				// if(minipc_rx.armor_id==0)
				// {
				// 	Yaw_Current_Value = PID_YAW_CAL_forward(&yaw_aoutpost_aim_PIDpos,&yaw_aoutpost_aim_PIDspeed,
				// 						yaw_angle_get,yaw_speed_get,yaw_angle_set,0.5,0.002,0.95);
				// 	Pitch_Current_Value = PID_PITCH_CAL_forward(&pitch_aoutpost_aim_PIDpos,&pitch_aoutpost_aim_PIDspeed,
				// 						pitch_angle_get,pitch_speed_get,pitch_angle_set ,0.5,0.002,0.5);						
				// }
				// else
				// {
	
						Yaw_Current_Value = PID_YAW_CAL_forward(&yaw_antitop_aim_PIDpos,&yaw_antitop_aim_PIDspeed,
										yaw_angle_get,yaw_speed_get,yaw_angle_set,0.1,0.002,1.5);
						Pitch_Current_Value = PID_PITCH_CAL_forward(&pitch_antitop_aim_PIDpos,&pitch_antitop_aim_PIDspeed,
										pitch_angle_get,pitch_speed_get,pitch_angle_set ,0.05,0.002,0.5);
				// }
			}break;
			default:break;
			}
		}	

		// Yaw_Current_Value= 2000.0*sin(0.020*sin_counter);
		// Yaw_Current_Value=100;
		GM6020Enableiq_4(&hcan1,pre4_id,(int16_t)Pitch_Current_Value,0,0,0);
		// GM6020Enableiq_4(&hcan2,pre4_id,(int16_t)Yaw_Current_Value,0,0,0);	
		DM4310Enable(&hcan2,pre4_id,(int16_t)Yaw_Current_Value,0,0,0);
		//记录云台上一次云台的模式
		gimbal_status.last_gimbal_mode = gimbal_status.gimbal_mode;
			
    	//任务周期唤醒2ms
    	osDelayUntil(&xLastWakeTime,Gimbal_control_Period);
  }
}

#endif// !GIMBAL_BOARD_DOWN



#ifdef GIMBAL_BOARD_UP
void gimbal_control_task(void const *pvParameters)
{
	static uint32_t cnt_last;
	static float dt_gimbal;
	//巡逻变量	
	static float pitch_patrol_w=5.0f; //rad/s  
	static float yaw_patrol_w=0.0026f;   
	static float time_all ;

	//控制器参数初始化

 	portTickType xLastWakeTime;
   	xLastWakeTime = xTaskGetTickCount();
  	/* Infinite loop */
  	for(;;)
  	{
		sin_counter++;
//		dt_gimbal = DWT_GetDeltaT (&cnt_last);
		count++;

   	 	gimbal_status.gimbal_mode =REMOTE_MODE;	
		//遥控器切换模式
		RemoteControlProcess(RC);	
		//云台模式切换 目标值更新
		Convermode_set(&gimbal_status);			//模式切换间过渡过程				
		//判断敌方是否小陀螺

		rotate_flag = top_status_judge(minipc_rx.Yaw_speed);
		rotate_direction = top_direction_judge(minipc_rx.Yaw_speed);
		gimbal_status.aimcenter_flag=aimcenter_judge(minipc_rx.Yaw_speed*1.2,6.2);

		switch(gimbal_status.gimbal_mode)
		{
			case REMOTE_MODE:
			{				
//				target_set.yaw_targetangle_remote=(300*sin(sin_counter*0.02))*coder2radian;					
				//pitch轴限幅(控制编码器)
				LimitPitch_encoder(&target_set,650,7850,motor_param_get[PITCH_MOTOR].offset_angle);		
				//pitch轴限幅(控制陀螺仪)
					
				/*yaw*/
				yaw_angle_set 						= target_set.yaw_targetangle_remote;
				yaw_angle_get						= INS.Yaw.total_radangle;
				//这里由于直接编码器的速度传感器值精度太差，采用陀螺仪的速度反馈值
				yaw_speed_get 						= INS.Yaw.speed;
				/*pitch*/
				pitch_angle_set 					= target_set.pitch_targetangle_remote;
				pitch_angle_get						= motor_param_get[PITCH_MOTOR].total_radangle;
				pitch_speed_get						= INS.Pitch.speed;
										
				Yaw_Current_Value=PID_YAW_CAL_forward(&yaw_rc_motor_PIDpos,&yaw_rc_motor_PIDspeed,
															yaw_angle_get,yaw_speed_get,yaw_angle_set,0,0.002,0);
				
				Pitch_Current_Value = PID_PITCH_CAL_forward(&pitch_rc_motor_PIDpos,&pitch_rc_motor_PIDspeed,
															pitch_angle_get,pitch_speed_get,pitch_angle_set,0,0.002,0);					
			}break;
			case PATROL_MODE:
			{
				if(gimbal_status.last_gimbal_mode ==PATROL_MODE )
				{
					//定义时间轴				
					time_all +=0.005;
					
					//期望值更新
//					target_set.yaw_targetangle_patrol=1.5*sinf(pitch_patrol_w*time_all*0.09);
					target_set.yaw_targetangle_patrol	 +=yaw_patrol_w;
					//清圈数
					if(fabsf(INS.Yaw.angle_round)>=1)
					{
			      		INS.Yaw.angle_round=0;
		     			//更新total 值			
		      			INS.Yaw.total_radangle=INS.Yaw.ins_angle*angle2radian;						
						target_set.yaw_targetangle_patrol=INS.Yaw.total_radangle;
					}
					target_set.pitch_targetangle_patrol=0.265*sinf(pitch_patrol_w*time_all*0.6);
//					target_set.pitch_targetangle_patrol=-0.12;
//					target_set.pitch_targetangle_patrol	 =(target_set.up_totalangle_encoderlimit-target_set.down_totalangle_encoderlimit)/2*sinf(pitch_patrol_w*time_all)+(target_set.up_totalangle_encoderlimit-(target_set.up_totalangle_encoderlimit-target_set.down_totalangle_encoderlimit)/2);				
				}
				//pitch轴限幅(控制编码器)
				LimitPitch_encoder(&target_set,650,7850,motor_param_get[PITCH_MOTOR].offset_angle);	
				/*yaw*/
				yaw_angle_set 						= target_set.yaw_targetangle_patrol;
				yaw_angle_get						= INS.Yaw.total_radangle;
				yaw_speed_get 						=	INS.Yaw.speed;
				/*pitch*/
				pitch_angle_set 					= target_set.pitch_targetangle_patrol	;
				pitch_angle_get						= motor_param_get[PITCH_MOTOR].total_radangle;
				pitch_speed_get						= INS.Pitch.speed;
					
				Yaw_Current_Value = PID_YAW_CAL_forward(&yaw_patrol_motor_PIDpos,&yaw_patrol_motor_PIDspeed,
												yaw_angle_get,yaw_speed_get,yaw_angle_set,
												0,0.002,0);
				Pitch_Current_Value = PID_PITCH_CAL_forward(&pitch_patrol_motor_PIDpos,&pitch_patrol_motor_PIDspeed,
											pitch_angle_get,pitch_speed_get,pitch_angle_set,
											0,0.002,0);				
			}break;
			case AUTO_AIM_MODE:
			{
				if(gimbal_status.last_gimbal_mode ==AUTO_AIM_MODE)	
				{					

					if(rotate_flag == 0 )
					{
						Woody_Autoaim(real_shoot_speed);		
					}
					else if(rotate_flag == 1 )
					{
						if(minipc_rx.armor_id==0)
						{
							Woody_Antioutpost(real_shoot_speed);
						}
						else
						{
							if (gimbal_status.aimcenter_flag==0) 
							{
								Woody_Antitop(real_shoot_speed);							
							}
							else 
							{
								Woody_Aimcentre(real_shoot_speed);
							}
						}
					}	
					
				}				
				else 
				{
					wait_armor = track_armor = 0;
				}
				// target_set.yaw_targetangle_gyro=(300*sin(sin_counter*0.02))*coder2radian;		
				// target_set.pitch_targetangle_gyro=0;				
				//PITCH轴限位
				LimitPitch_gyro(&target_set,650*coder2radian,-362*coder2radian);
										
				/*yaw*/         
				yaw_angle_set            	= target_set.yaw_targetangle_gyro;
				yaw_angle_get			 	= INS.Yaw.total_radangle ;
				yaw_speed_get 				= INS.Yaw.speed;
				/*pitch*/			
				pitch_angle_set          	= target_set.pitch_targetangle_gyro ;
				pitch_angle_get				= INS.Pitch.total_radangle ;
				pitch_speed_get				= INS.Pitch.speed;
	
							
				if(rotate_flag ==0)	
				{
					Yaw_Current_Value = PID_YAW_CAL_forward(&yaw_auto_aim_PIDpos,&yaw_auto_aim_PIDspeed,
															yaw_angle_get,yaw_speed_get,yaw_angle_set,0.05,0.002,0.5);
					Pitch_Current_Value = PID_PITCH_CAL_forward(&pitch_auto_aim_PIDpos,&pitch_auto_aim_PIDspeed,
											pitch_angle_get,pitch_speed_get,pitch_angle_set  ,0.05,0.002,0.62);				
				}
				else if(rotate_flag==1 )
				{
					if(minipc_rx.armor_id==0)
					{
						Yaw_Current_Value = PID_YAW_CAL_forward(&yaw_aoutpost_aim_PIDpos,&yaw_aoutpost_aim_PIDspeed,
															yaw_angle_get,yaw_speed_get,yaw_angle_set,0.1,0.002,1.0);
					 	Pitch_Current_Value = PID_PITCH_CAL_forward(&pitch_aoutpost_aim_PIDpos,&pitch_aoutpost_aim_PIDspeed,
											pitch_angle_get,pitch_speed_get,pitch_angle_set ,0.05,0.002,0.6);				
					}
					else
					{
						Yaw_Current_Value = PID_YAW_CAL_forward(&yaw_antitop_aim_PIDpos,&yaw_antitop_aim_PIDspeed,
															yaw_angle_get,yaw_speed_get,yaw_angle_set,0.1,0.002,1.5);
					 	Pitch_Current_Value = PID_PITCH_CAL_forward(&pitch_antitop_aim_PIDpos,&pitch_antitop_aim_PIDspeed,
											pitch_angle_get,pitch_speed_get,pitch_angle_set ,0.05,0.002,0.65);						
					}
					
				}					
			}break;
			default:break;
		}
			
		//电流发送函数
//			t=1500*sin(0.005*sin_counter);
//			Yaw_Current_Value= 1500.0/16384*7.0*sin(0.005*sin_counter);
			// Yaw_Current_Value= 2000.0*sin(0.020*sin_counter);	


		GM6020Enableiq_4(&hcan1,pre4_id,(int16_t)Pitch_Current_Value,0,0,0);	
		// // ctrl_motor(&hcan1,0x2, 0, 0, 0, 0,Yaw_Current_Value);
		// // ctrl_motor(&hcan1,0x2, 0, 0, 0, 0,0.0);
		DM4310Enable(&hcan1,pre4_id,0,(int16_t)Yaw_Current_Value,0,0);		
		// DM4310Enable(&hcan1,pre4_id,0,0,0,0);		
		//记录e云台上一次云台的模式
		gimbal_status.last_gimbal_mode = gimbal_status.gimbal_mode;

    	//任务周期唤醒2ms
   	 	osDelayUntil(&xLastWakeTime,Gimbal_control_Period);
  }
}
#endif//!GIMBAL_BOARD_UP








